import os
import ctypes
import sys
sys.path.append('/home/pi/MasterPi/')
import time
import HiwonderSDK.Board
from smbus2 import SMBus, i2c_msg



if sys.version_info.major == 2:
    print('Please run this program with python3!')
    sys.exit(0)

class Gyro:
 
    def __init__(self):
        self.i2c_addr = 0x50
        self.i2c = 1

    def __getattr(self, attr):
        if attr in self.__units:
            return self.__units[attr]
        if attr == "GyroData":
            return self.getGryoData()
        else:
            raise AttributeError('Unknow attribute : %s'%attr)
 

    def getGryoData(self):
        allGyroData = []
        try:
            with SMBus(self.i2c) as bus:

                speedData = bus.read_i2c_block_data(self.i2c_addr,0x37,18)
                speedX = ctypes.c_int16(speedData[1]<<8 | speedData[0]).value/32768.0*2000.0
                speedY = ctypes.c_int16(speedData[3]<<8 | speedData[2]).value/32768.0*2000.0
                speedZ = ctypes.c_int16(speedData[5]<<8 | speedData[4]).value/32768.0*2000.0
                allGyroData.append(speedX)
                allGyroData.append(speedY)
                allGyroData.append(speedZ)
                angleData = bus.read_i2c_block_data(self.i2c_addr,0x3D,6)
                
                angleX = ctypes.c_int16(angleData[1]<<8 | angleData[0]).value/32768.0*180.0
                angleY = ctypes.c_int16(angleData[3]<<8 | angleData[2]).value/32768.0*180.0
                angleZ = ctypes.c_int16(angleData[5]<<8 | angleData[4]).value/32768.0*180.0
                allGyroData.append(angleX)
                allGyroData.append(angleY)
                allGyroData.append(angleZ)
       
        except BaseException as e:
            pass
        return allGyroData

if __name__ == '__main__':
    s = Gyro()

    time.sleep(0.1)
    while True:
        time.sleep(0.1)
        gyroData = s.getGryoData()
        print(gyroData[5])

